﻿using System;
using System.Collections.Generic;
using System.Diagnostics; // DeviceListItem, ServoStatus
using System.Linq;
using System.Timers;
using System.Text;
using System.Threading;
using Pololu.UsbWrapper; // Usc
using Pololu.Usc;

using MySettings = AMEE.PanTiltControl.Properties.Settings;

namespace AMEE.PanTiltControl
{
    /// <summary>
    /// Steuerung der PanTilt Einheit (AMEE Hals).
    /// </summary>
    public class PanTilt : IDisposable
    {
        

        /// <summary>Maestro PanTilt Controler</summary>
        Usc usc = null;
        ServoStatus[] _servos;
        static readonly int TILT_ID, PAN_ID;
        static readonly byte TILT_ID_B, PAN_ID_B;
        static readonly float PI_Half = (float)(Math.PI / 2.0);
        /// <summary>absolute difference between 0 and 90 degres</summary>
        static readonly float PAN90;
        static readonly float TILT90;
        /// <summary>Deviation (+/-) from Position, that will interpreted as targeted.</summary>
        static readonly int _DEVIATION = 20;
        bool _isDisposed;

        /// <summary>The minimum target value of the Tilt unit defined at the config file.</summary>
        public static readonly int TILT_MIN;
        /// <summary>The maximum target value of the Tilt unit defined at the config file.</summary>
        public static readonly int TILT_MAX;
        /// <summary>The middle / neutral target value of the Tilt unit defined at the config file.</summary>
        public static readonly int TILT_NEUTRAL;
        /// <summary>The minimum target value of the Pan unit defined at the config file.</summary>
        public static readonly int PAN_MIN;
        /// <summary>The maximum target value of the Pan unit defined at the config file.</summary>
        public static readonly int PAN_MAX;
        /// <summary>The middle / neutral target value of the Pan unit defined at the config file.</summary>
        public static readonly int PAN_NEUTRAL;


        #region Properties

        /// <summary>
        /// Gets the serial number.
        /// </summary>
        /// <value>
        /// The serial number.
        /// </value>
        public String SerialNumber { get; protected set; }

        //TODO Property Position als Int32 oder UInt16 anbieten?
        int _targetPan;
        /// <summary>
        /// Gets or sets the Pan target.<para>
        /// Setting value lets the Pan unit move. Getting Target returns last set target.</para>
        /// </summary>
        /// <value>
        /// The Pan target position.
        /// </value>
        public int TargetPan {
            get { return _targetPan; }
            set {
                if (value >= UInt16.MinValue && value <= (UInt16.MaxValue)) {
                    if (value < PanTilt.PAN_MIN) {
                        Debug.WriteLine("Pan value= " + value + " is lower than MinValue= " + PanTilt.PAN_MIN, "Warning");
                        return;
                    }
                    if (value > PanTilt.PAN_MAX) {
                        Debug.WriteLine("Pan value= " + value + " is greater than MaxValue= " + PanTilt.PAN_MAX, "Warning");
                        return;
                    }
                    _targetPan = value;
                    // Set Target:
                    usc.setTarget(PanTilt.PAN_ID_B, (ushort)_targetPan);
                    Debug.WriteLine("pan set target(" + value + ")", "Info");
                }
                else
                    Debug.WriteLine("Pan value " + value + "is not an UInt16", "Warning");
            }
        }

        int _targetTilt;
        /// <summary>
        /// Gets or sets the Tilt target position.<para>
        /// Setting value lets the Tilt unit move. Getting Target returns last or set Target.</para>
        /// </summary>
        /// <value>
        /// The tilt target position.
        /// </value>
        public int TargetTilt {
            get { return _targetTilt; }
            set {
                if (value >= UInt16.MinValue && value <= (UInt16.MaxValue)) {
                    if (value < PanTilt.TILT_MIN) {
                        Debug.WriteLine("Tilt value= " + value + " is lower than MinValue= " + PanTilt.TILT_MIN, "Warning");
                        return;
                    }
                    if (value > PanTilt.TILT_MAX) {
                        Debug.WriteLine("Tilt value= " + value + " is greater than MaxValue= " + PanTilt.TILT_MAX, "Warning");
                        return;
                    }
                    _targetTilt = value;
                    // Set Target:
                    usc.setTarget(PanTilt.TILT_ID_B, (ushort)_targetTilt);
                    Debug.WriteLine("tilt set target(" + value + ")", "Info");
                }
                else
                    Debug.WriteLine("Tilt value " + value + "is not an UInt16", "Warning");
            }
        }

        /// <summary>
        /// Gets the current device's Pan position .
        /// </summary>
        public int PositionPan { 
            get {
                usc.getVariables(out this._servos);
                return _servos[PanTilt.PAN_ID].position; 
            }
        }

        /// <summary>
        /// Gets the pan position in radiant.
        /// </summary>
        public float PositionRadPan {
            get {
                float retVal = (PositionPan - PanTilt.PAN_NEUTRAL) / PanTilt.PAN90 * PanTilt.PI_Half;
                return retVal;
            }
        }

        /// <summary>
        /// Gets the tilt orientation in radiant.
        /// </summary>
        public float PositionRadTilt {
            get {
                float retVal = (PositionTilt - PanTilt.TILT_NEUTRAL) / PanTilt.TILT90 * PanTilt.PI_Half;
                return retVal;
            }
        }

        //int _positionTilt;
        /// <summary>
        /// Gets the current device's Tilt position .
        /// </summary>
        public int PositionTilt { 
            get {
                usc.getVariables(out this._servos);
                return _servos[PanTilt.TILT_ID].position;
            }
            //private set {

            //}
        }


        #endregion

        #region Constructor

        /// <summary>
        /// Initializes a new instance of the <see cref="PanTilt"/> class.
        /// </summary>
        /// <exception cref="AMEE.PanTiltControl.PanTiltControlException">Could not connect to Controller</exception>
        public PanTilt() {
            // Properties.Settings.Default.ServoCtrlID enthält die ID unseres Controllers
            // Optional: Konstruktor oder connect Methode mit Nutzerspezifizierter ID
            bool isConnected = connect();
            if (!isConnected)
                throw new PanTiltControlException("Could not connect to Controller");
        }


        static PanTilt() {
            PanTilt.PAN_ID = Properties.Settings.Default.PanPort;
            PanTilt.PAN_ID_B = (byte)PanTilt.PAN_ID;
            PanTilt.TILT_ID = Properties.Settings.Default.TiltPort;
            PanTilt.TILT_ID_B = (byte)PanTilt.TILT_ID;

            PanTilt.PAN_MIN = Properties.Settings.Default.PanMin;
            PanTilt.PAN_MAX = Properties.Settings.Default.PanMax;
            PanTilt.PAN_NEUTRAL = Properties.Settings.Default.PanNeutral;
            PanTilt.TILT_MIN = Properties.Settings.Default.TiltMin;
            PanTilt.TILT_MAX = Properties.Settings.Default.TiltMax;
            PanTilt.TILT_NEUTRAL = Properties.Settings.Default.TiltNeutral;

            PanTilt.PAN90 = MySettings.Default.PanNeutral - MySettings.Default.Pan90Minus;
            PanTilt.TILT90 = MySettings.Default.TiltNeutral - MySettings.Default.Tilt90Minus;
        }

        /// <summary>
        /// Finalizes an instance of the <see cref="PanTilt"/> class.
        /// </summary>
        ~PanTilt() {
            this.Dispose(false);
        }

        #endregion
        

        #region IDisposable Members

        /// <summary>
        /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources.
        /// </summary>
        public void Dispose() {
            Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Releases unmanaged and - optionally - managed resources.
        /// </summary>
        /// <param name="disposing"><c>true</c> to release both managed and unmanaged resources; <c>false</c> to release only unmanaged resources.</param>
        protected virtual void Dispose(bool disposing) {
            if (!this._isDisposed) {
                if (disposing) {
                    // release managed ressouces
                }
                this.tryToDisconnect();
                this._isDisposed = true;
            }
        }

        #endregion

        /// <summary>
        /// Wait until target positions are reached.
        /// </summary>
        
        public void Wait() {
            this.Wait(Int32.MaxValue);
        }

        /// <summary>
        /// Waits (Blocks) until target positions or the specified timeout in ms are reached.
        /// </summary>
        /// <param name="timeout">The timeout.</param>
        /// <returns>true, if Positions are reched. False otherwise (at timeout)</returns>
        /// <remarks>
        /// Updates and checks every 50 ms the device's Positions
        /// </remarks>
        public bool Wait(int timeout) {
            int cnt = 0;
            int currTilt;
            int currPan;
            bool isOnTarget = false;
            do {
                usc.getVariables(out this._servos);
                currTilt = _servos[PanTilt.TILT_ID].position;
                currPan = _servos[PanTilt.PAN_ID].position; ;
                isOnTarget = (currPan >= (TargetPan - PanTilt._DEVIATION)) && (currPan <= (TargetPan + PanTilt._DEVIATION));
                isOnTarget = isOnTarget && (currTilt >= (TargetTilt - PanTilt._DEVIATION)) && (currTilt <= (TargetTilt + PanTilt._DEVIATION));
                //usc.setTarget(PanTilt.PAN_ID_B, (ushort)panTarget);
                //usc.setTarget(PanTilt.TILT_ID_B, (ushort)tiltTarget);
                Thread.Sleep(50);
                cnt += 50;
            } while (! (isOnTarget || cnt >= timeout));
            Debug.WriteLine("Wait finished with " + (isOnTarget ? "target reached.":"timeout."),"Info");
            return isOnTarget;
        }

        /// <summary>
        /// Let the device move the specified targets in radiant.
        /// </summary>
        /// <param name="panTargetRad">The pan target in radiant.</param>
        /// <param name="tiltTargetRad">The tilt target in radiant.</param>
        public void Move(float panTargetRad, float tiltTargetRad) {
            float p = MySettings.Default.PanNeutral + (PanTilt.PAN90 * panTargetRad / PanTilt.PI_Half);
            float t = MySettings.Default.TiltNeutral + (PanTilt.TILT90 * tiltTargetRad / PanTilt.PI_Half);
            TargetPan = (int)p;
            TargetTilt = (int)t;
        }


        #region the Maestro connection - copy from MeastroAdvancedExcample

        /// <summary>
        /// Connects this instance.
        /// </summary>
        /// <returns>True, if Meastro Device was found. False otherwise</returns>
        bool connect() {
            // Get the serial number of the first connected Maestro, since this is
            // probably what you want to connect to.
            var device_list = Usc.getConnectedDevices();
            if (device_list.Count > 0) {
                SerialNumber = device_list[0].serialNumber;
                usc = new Usc(device_list[0]); //Note: wirft Exception, wenn Projekt nicht in NET 3.5
                ServoStatus[] servos;
                usc.getVariables(out servos);

                return true;
            }
            return false;
        }

        ///// <summary>
        ///// Connects to the device if it is found in the device list.
        ///// </summary>
        //void tryToReconnect() {
        //    foreach (DeviceListItem d in Usc.getConnectedDevices()) {
        //        if (d.serialNumber == SerialNumber) {
        //            usc = new Usc(d);
        //            Debug.WriteLine("Connected to #" + SerialNumber + ".");
        //            return;
        //        }
        //    }
        //}


        void tryToDisconnect() {
            if (usc == null) {
                Debug.WriteLine("Connecting stopped.");
                return;
            }

            try {
                Debug.WriteLine("Disconnecting...");
                usc.Dispose();  // Disconnect
            }
            catch (Exception e) {
                Debug.WriteLine(e);
                Debug.WriteLine("Failed to disconnect cleanly.");
            }
            finally {
                // do this no matter what
                usc = null;
                Debug.WriteLine("Disconnected from #" + SerialNumber + ".");
            }
        }



        #endregion


        
    }
}
